#include "PL2219C.h" 
#include "software_i2c.h"




#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include "stdlib.h"
 
 #define FLITER_ENABLE 0
 
#define  _CLIFF_PIN1    GPIO_PIN_4   //PA4
#define  _CLIFF_PIN2    GPIO_PIN_5   //PA5
#define  _CLIFF_PIN3    GPIO_PIN_6   //PA6
#define  _CLIFF_PIN4    GPIO_PIN_7   //PA7

#define  INIT_ALL_PIN_LEVEL  GPIO_PIN_SET
 
#define  Trig_CLIFF_PIN_LEVEL       GPIO_PIN_SET 

#define  UnTrig_CLIFF_PIN_LEVEL     !Trig_CLIFF_PIN_LEVEL 


static void Set_Out_PIN(uint16_t pins,bool level)  // ��������״̬
{
		HAL_GPIO_WritePin(GPIOA,pins,level == true ? GPIO_PIN_SET : GPIO_PIN_RESET);
}

 
 
 PL2219C_t   PL2219C = {
 
	  .PL2219C[0]={
		
			.pins = _CLIFF_PIN1,
			.Set_Out = Set_Out_PIN
		},
		.PL2219C[1]={
		
			.pins = _CLIFF_PIN2,
			.Set_Out = Set_Out_PIN
		},
		.PL2219C[2]={
			.pins = _CLIFF_PIN3,
			.Set_Out = Set_Out_PIN
		},
		.PL2219C[3]={
		
			.pins = _CLIFF_PIN4,
			.Set_Out = Set_Out_PIN
		},
		 
	 
 };
 



 #define FILTER_SIZE 10  // Size of the filter buffer

// Struct to hold the filtered values for proximity and RGB
typedef struct {

	uint16_t ir_data_buffer[FILTER_SIZE];
    uint16_t proximity_data_buffer[FILTER_SIZE];
    uint16_t red_light_buffer[FILTER_SIZE];
    uint16_t green_light_buffer[FILTER_SIZE];
    uint16_t blue_light_buffer[FILTER_SIZE];
    uint8_t filter_index;
    uint8_t filter_count;
} PL2219C_Filter;

PL2219C_Filter pl2219c_filter = { {0}, {0}, {0}, {0}, 0, 0 };

// Simple moving average filter function for uint16_t
uint16_t moving_average(uint16_t new_value, uint16_t* buffer) {
    buffer[pl2219c_filter.filter_index] = new_value;
    pl2219c_filter.filter_index = (pl2219c_filter.filter_index + 1) % FILTER_SIZE;
    
    // If the filter hasn't been fully populated yet, we just return the new value (no averaging yet)
    if (pl2219c_filter.filter_count < FILTER_SIZE) {
        pl2219c_filter.filter_count++;
        return new_value;
    }

    uint32_t sum = 0;
    for (int i = 0; i < FILTER_SIZE; i++) {
        sum += buffer[i];
    }

    return sum / FILTER_SIZE;
}


#if 0
bool Get_PS(uint16_t * PS){


	uint8_t buffer[2] = {0};


	 memset(buffer,0x00,sizeof(buffer));
	 if( (!wireReadDataByte(0xAA,&buffer[0])) ){

		return false;
	 }

	 if( (!wireReadDataByte(0xAB,&buffer[1])) ){

		return false;
	 }

	* PS =  buffer[0] |  buffer[1]<<8;


	return true;
}


bool Get_RGB(uint16_t * R,uint16_t *G,uint16_t *B){



	uint8_t buffer[2] = {0};


	 memset(buffer,0x00,sizeof(buffer));
	 if( (!wireReadDataByte(0xA0,&buffer[0])) ){

		return false;
	 }

	 if( (!wireReadDataByte(0xA1,&buffer[1])) ){

		return false;
	 }

	
	
	
	* R =  buffer[0] |  buffer[1]<<8;




		 memset(buffer,0x00,sizeof(buffer));
	 if( (!wireReadDataByte(0xA2,&buffer[0])) ){

		return false;
	 }

	 if( (!wireReadDataByte(0xA3,&buffer[1])) ){

		return false;
	 }


* G =  buffer[0] |  buffer[1]<<8;

	 	 memset(buffer,0x00,sizeof(buffer));
	 if( (!wireReadDataByte(0xA4,&buffer[0])) ){

		return false;
	 }

	 if( (!wireReadDataByte(0xA5,&buffer[1])) ){

		return false;
	 }


* B =  buffer[0] |  buffer[1]<<8;

	 return true;
}
#else

bool Get_IR(uint16_t *IR) {
    uint8_t buffer[2] = {0};

    memset(buffer, 0x00, sizeof(buffer));
    if ((!wireReadDataByte(0xA8, &buffer[0]))) {
        return false;
    }

    if ((!wireReadDataByte(0xA9, &buffer[1]))) {
        return false;
    }

    *IR = buffer[0] | buffer[1] << 8;

	#if FLITER_ENABLE
    // Apply moving average filter to the proximity data
    *IR = moving_average(*IR, pl2219c_filter.ir_data_buffer);
	#endif
    return true;
}


bool Get_PS(uint16_t *PS) {
    uint8_t buffer[2] = {0};

    memset(buffer, 0x00, sizeof(buffer));
    if ((!wireReadDataByte(0xAA, &buffer[0]))) {
        return false;
    }

    if ((!wireReadDataByte(0xAB, &buffer[1]))) {
        return false;
    }

    *PS = buffer[0] | buffer[1] << 8;

	#if FLITER_ENABLE
    // Apply moving average filter to the proximity data
    *PS = moving_average(*PS, pl2219c_filter.proximity_data_buffer);
	#endif

    return true;
}

bool Get_RGB(uint16_t *R, uint16_t *G, uint16_t *B) {
    uint8_t buffer[2] = {0};

    memset(buffer, 0x00, sizeof(buffer));
    if ((!wireReadDataByte(0xA0, &buffer[0]))) {
        return false;
    }

    if ((!wireReadDataByte(0xA1, &buffer[1]))) {
        return false;
    }

    *R = buffer[0] | buffer[1] << 8;

	#if FLITER_ENABLE
    *R = moving_average(*R, pl2219c_filter.red_light_buffer);  // Apply filter
	#endif
    memset(buffer, 0x00, sizeof(buffer));
    if ((!wireReadDataByte(0xA2, &buffer[0]))) {
        return false;
    }

    if ((!wireReadDataByte(0xA3, &buffer[1]))) {
        return false;
    }

    *G = buffer[0] | buffer[1] << 8;

	#if FLITER_ENABLE
    *G = moving_average(*G, pl2219c_filter.green_light_buffer);  // Apply filter
	#endif

    memset(buffer, 0x00, sizeof(buffer));
    if ((!wireReadDataByte(0xA4, &buffer[0]))) {
        return false;
    }

    if ((!wireReadDataByte(0xA5, &buffer[1]))) {
        return false;
    }

    *B = buffer[0] | buffer[1] << 8;

	#if 0
    *B = moving_average(*B, pl2219c_filter.blue_light_buffer);  // Apply filter
	#endif
    return true;
}

#endif



static uint8_t scale_uint16_to_uint8(uint16_t value) {
    // ����ӳ�乫ʽ�� (value / 65535.0) * 255
    return (uint8_t)((value * 255) / 4096);
}

 
bool PL2219C_Init_Config(){




uint8_t buffer[2] = {0};



	 if( (!wireReadDataByte(0x98,&buffer[0])) ){

		return false;
	 }

	 printf("read reg %02x\r\n",buffer[0]);

	 if(buffer[0] != 0xA0){

		return false;
	 }


	 	if( ! wireWriteDataByte(0x81,0x02) ){
		return false;	
	 }


	 	 if( ! wireWriteDataByte(0x82,0x57) ){
		return false;	
	 }

	 	 if( ! wireWriteDataByte(0x83,0x01) ){
		return false;	
	 }

	 	 if( ! wireWriteDataByte(0x84,0x07) ){
		return false;	
	 }

	 	 if( ! wireWriteDataByte(0x85,0x0F) ){
		return false;	
	 }

	 	 if( ! wireWriteDataByte(0x86,0x06) ){
		return false;	
	 }

	 	 if( ! wireWriteDataByte(0x87,0x1E) ){
		return false;	
	 }

		//设置增益
	//  	 if( ! wireWriteDataByte(0x89,0x38) ){
	// 	return false;	
	//  }

	// 	 if( ! wireWriteDataByte(0x89,0x3B) ){
	// 	return false;	
	//  }
		 if( ! wireWriteDataByte(0x89,0x0B) ){
		return false;	
	 }

		if( ! wireWriteDataByte(0x80,0x1F) ){
		return false;	
	 }
 


	#if 0
	R_Senser_ID = 0x72;
	IIC_Start();						
	IIC_WriteByte(R_Senser_ID);
	IIC_ACK();
	IIC_WriteByte(0xA0);
	IIC_ACK();
	IIC_Start();
	IIC_WriteByte(R_Senser_ID | 0x01);
	IIC_ACK();
	u8_R_ValueL = IIC_ReadByte();
	IIC_MACK();
	u32_R_Value = IIC_ReadByte();
	IIC_MACK();
	u8_G_ValueL = IIC_ReadByte();
	IIC_MACK();
	u32_G_Value = IIC_ReadByte();
	IIC_MACK();
	u8_B_ValueL = IIC_ReadByte();
	IIC_MACK();
	u32_B_Value = IIC_ReadByte();
	IIC_MACK();
	u8_C_ValueL = IIC_ReadByte();
	IIC_MACK();
	u32_C_Value = IIC_ReadByte();
	IIC_MACK();
	u8_IR_ValueL = IIC_ReadByte();
	IIC_MACK();
	u32_IR_Value = IIC_ReadByte();
	IIC_MACK();
	u8_PS_ValueL = IIC_ReadByte();
	IIC_MACK();
	u32_PS_Value = IIC_ReadByte();
	IIC_NACK();
	IIC_Stop();
	u32_R_Value <<= 8;
	u32_R_Value |= u8_R_ValueL;
	u32_G_Value <<= 8;
	u32_G_Value |= u8_G_ValueL;
	u32_B_Value <<= 8;
	u32_B_Value |= u8_B_ValueL;
	u32_C_Value <<= 8;
	u32_C_Value |= u8_C_ValueL;
	u32_IR_Value <<= 8;
	u32_IR_Value |= u8_IR_ValueL;
	u32_PS_Value <<= 8;
	u32_PS_Value |= u8_PS_ValueL;
	__NOP();
	#endif
	return true;
}


void PL2219C_Init(){
	
	
	
	//__HAL_RCC_GPIOA_CLK_ENABLE();                          /* Enable GPIOA clock */

	GPIO_InitTypeDef  gpio_init;
	gpio_init.Pin = GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7;
	gpio_init.Mode = GPIO_MODE_OUTPUT_OD;            /* Push-pull output */
	gpio_init.Pull = GPIO_PULLUP;                    /* Enable pull-up */
	gpio_init.Speed = GPIO_SPEED_FREQ_VERY_HIGH;          /* GPIO speed */  
	HAL_GPIO_Init(GPIOA, &gpio_init);
	HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4|GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7,INIT_ALL_PIN_LEVEL);

	



		

		for(int i = 1;i < 2;i++){
			
			  HAL_Delay(200);

			  PL2219C.i2c_id = i;
			  PL2219C.PL2219C[i].id_num = i;
			  PL2219C.PL2219C[i].is_init  = PL2219C_Init_Config();//PL2219_MP_Initial_Config();
			  PL2219C.PL2219C[i].status = false;
			  PL2219C.PL2219C[i].last_status = false;
			
			  printf("PL2219C num %d is %d,addr %02x\r\n",i,PL2219C.PL2219C[i].is_init,PL2219C.addr);
		
		}

			  //PL2219C.addr = PL2219C.addr + 1;
			  //PL2219C.addr = PL2219C.addr % 256;	
}



void PL2219C_Task_Entry(_PL2219C_t *PL2219C) {


		if(PL2219C->proximity_data < PROXIMITY_2219C_THREASOLD){
					
					

						
				if( (PL2219C->red_light < COLOR_2219C_THREASOLD) && (PL2219C->green_light < COLOR_2219C_THREASOLD) && (PL2219C->blue_light < COLOR_2219C_THREASOLD) ){

									//printf("black\r\n");
									PL2219C->Set_Out(PL2219C->pins,UnTrig_CLIFF_PIN_LEVEL);
								
									//return;
				 }else{
									//printf("no black\r\n");
									PL2219C->Set_Out(PL2219C->pins,Trig_CLIFF_PIN_LEVEL);		
				 }
				 
		}else{

			//printf("whilte\r\n");
			PL2219C->Set_Out(PL2219C->pins,UnTrig_CLIFF_PIN_LEVEL);

		}
				
				
				
	
}



void PL2219C_Task(){


	
	while(1){
		
		
					for(int i = 1;i<2;i++){
							
									if(PL2219C.PL2219C[i].is_init)
									{
									
											PL2219C.i2c_id = i;
											printf("PL2219C.i2c_id = %d ",PL2219C.i2c_id);

											if(Get_PS(&PL2219C.PL2219C[i].proximity_data)){
													//printf("1\r\n");
													if(Get_RGB(&PL2219C.PL2219C[i].red_light,&PL2219C.PL2219C[i].green_light,&PL2219C.PL2219C[i].blue_light))
													
													{
													//printf("1\r\n");


													
//															printf("ps %d,color %d,%d,%d\r\n",PL2219C.PL2219C[i].proximity_data,
//																PL2219C.PL2219C[i].red_light,
//																PL2219C.PL2219C[i].green_light,
//																PL2219C.PL2219C[i].blue_light
//															);


																// if(Get_IR(&PL2219C.PL2219C[i].ir_data)){




																// PL2219C.PL2219C[i].red_light = scale_uint16_to_uint8(PL2219C.PL2219C[i].red_light);
															 	// PL2219C.PL2219C[i].green_light = scale_uint16_to_uint8(PL2219C.PL2219C[i].green_light);
															 	// PL2219C.PL2219C[i].blue_light = scale_uint16_to_uint8(PL2219C.PL2219C[i].blue_light);

																printf("ps %d,ir %d,color %d,%d,%d\r\n",(PL2219C.PL2219C[i].proximity_data),
																 PL2219C.PL2219C[i].ir_data,
															 	(PL2219C.PL2219C[i].red_light),
															 	(PL2219C.PL2219C[i].green_light),
															 	(PL2219C.PL2219C[i].blue_light)
															 );


															// 			printf("ps %d\r\n",(PL2219C.PL2219C[i].proximity_data)
															//  );





																// PL2219C.PL2219C[i].red_light = scale_uint16_to_uint8(PL2219C.PL2219C[i].red_light);
															 	// PL2219C.PL2219C[i].green_light = scale_uint16_to_uint8(PL2219C.PL2219C[i].green_light);
															 	// PL2219C.PL2219C[i].blue_light = scale_uint16_to_uint8(PL2219C.PL2219C[i].blue_light);

																	PL2219C_Task_Entry(&PL2219C.PL2219C[i]);

																// }

															 	
													}
											}

											
									}
					}
	}
	
	
	

}



/*******************************************************************************
 * Raw I2C Reads and Writes
 ******************************************************************************/

/**
 * @brief Writes a single byte to the I2C device (no register)
 *
 * @param[in] val the 1-byte value to write to the I2C device
 * @return True if successful write operation. False otherwise.
 */
static bool  wireWriteByte(uint8_t val)
{
    // Wire.beginTransmission(APDS9960_I2C_ADDR);
    // Wire.write(val);
    // if( Wire.endTransmission() != 0 ) {
    //     return false;
    // }


//    uint8_t data[1] = {0}; // ����һ����Ч�Ŀ���������
//    esp_err_t esp_err;
//    data[0] = val;
//    esp_err = i2c_master_write_to_device(I2C_MASTER_NUM, APDS9960_I2C_ADDR, data, sizeof(data), 1000 / portTICK_PERIOD_MS);   /*   SW reset   */

//     if (esp_err == ESP_OK) {

//        return true;
//     }
	
	
	  uint8_t data[1] = {0}; // ����һ����Ч�Ŀ���������
    data[0] = val;
		
	return I2C_Write(PL2219C.i2c_id,PL2219C_I2C_ADDR, 0x00,data,1);
    
    //return false;
}

/**
 * @brief Writes a single byte to the I2C device and specified register
 *
 * @param[in] reg the register in the I2C device to write to
 * @param[in] val the 1-byte value to write to the I2C device
 * @return True if successful write operation. False otherwise.
 */
static bool  wireWriteDataByte(uint8_t reg, uint8_t val)
{

		uint8_t data[1] = {0}; // ����һ����Ч�Ŀ���������
    data[0] = val;
		
	return I2C_Write(PL2219C.i2c_id,PL2219C_I2C_ADDR, reg,data,1);
    
}

/**
 * @brief Writes a block (array) of bytes to the I2C device and register
 *
 * @param[in] reg the register in the I2C device to write to
 * @param[in] val pointer to the beginning of the data byte array
 * @param[in] len the length (in bytes) of the data to write
 * @return True if successful write operation. False otherwise.
 */
static bool  wireWriteDataBlock(  uint8_t reg, 
                                        uint8_t *val, 
                                        unsigned int len)
{
   
	return I2C_Write(PL2219C.i2c_id,PL2219C_I2C_ADDR, reg,val,len);
}

/**
 * @brief Reads a single byte from the I2C device and specified register
 *
 * @param[in] reg the register to read from
 * @param[out] the value returned from the register
 * @return True if successful read operation. False otherwise.
 */
static bool  wireReadDataByte(uint8_t reg, uint8_t *val)
{
    return I2C_Read(PL2219C.i2c_id,PL2219C_I2C_ADDR, reg,val,1);
}

/**
 * @brief Reads a block (array) of bytes from the I2C device and register
 *
 * @param[in] reg the register to read from
 * @param[out] val pointer to the beginning of the data
 * @param[in] len number of bytes to read
 * @return Number of bytes read. -1 on read APDS9960_ERROR.
 */
static int  wireReadDataBlock(   uint8_t reg, 
                                        uint8_t *val, 
                                        unsigned int len)
{
    unsigned char i = 0;
    
    /* Indicate which register we want to read from */
    // if (!wireWriteByte(reg)) {
    //     return -1;
    // }
    /* Read block data */
    // Wire.requestFrom(APDS9960_I2C_ADDR, len);
    // while (Wire.available()) {
    //     if (i >= len) {
    //         return -1;
    //     }
    //     val[i] = Wire.read();
    //     i++;
    // }
//        uint8_t data[2] = {0}; // ����һ����Ч�Ŀ���������
//    esp_err_t esp_err;
//    data[0] = reg;
//        esp_err = i2c_master_write_read_device(I2C_MASTER_NUM,APDS9960_I2C_ADDR, data, 1, val, len, 1000 / portTICK_PERIOD_MS);
//        if (esp_err == ESP_OK) {

//            //val = data[1];
//            return len;
//             //printf("3 Found I2C device at address 0x%02X\n",data[1]);
//        }



	 return I2C_Read(PL2219C.i2c_id,PL2219C_I2C_ADDR, reg,val,len);


    return -1;
}